package ecprac.drivers.def;

import ecprac.ea.abstracts.AbstractDriver;
import ecprac.ea.abstracts.map.Corner;
import ecprac.torcs.client.Action;
import ecprac.torcs.client.SensorModel;
import ecprac.torcs.genome.IGenome;

public class DefaultDriver extends AbstractDriver {

	private DefaultDriverGenome[] genes;
	double c0 = 0.5;
	double c1 = 0.5;
	public double[] fitness;
	public DriverChromosome DC;
	public int index;

	private int counter = 0;
	private double lastPos = 0;
	private double posStart = 0;

	private double startTime = 0;



	private String driverName;

	DefaultDriver (String name, int index) {
		driverName = name;
		this.index = index;
		fitness = new double[DefaultEA.clusters.length];
	}

	public void loadGenome(IGenome genome) {

		if (genome instanceof DriverChromosome) {
			DriverChromosome llgenome = (DriverChromosome) genome;
			DC = llgenome;

			genes = llgenome.genes;
			// TODO: Remove this when Steering is implemented
			c0 = llgenome.genes[0].c0;
			c1 = llgenome.genes[0].c1;
		} else {
			System.err.println("Invalid Genome assigned");
		}

	}

	/**
	 * Returns a number. 1 = accelerate, -1 = decelerate.
	 */
	public double getAcceleration(SensorModel sensors){
		int check = isCalm(sensors);
		if (check != 0)
			return check;
		int cluster = getCornerCluster(sensors);
		counter++;
		Corner c = trackmap.getNextCorner(sensors.getDistanceFromStartLine());	
		double cornerStart = c.location - c.length/2;
		double carLocation = sensors.getDistanceFromStartLine();
		// Takes care of timing, and therefore fitness
		if (c.index > 0) {
			Corner c2 = trackmap.getCorner(c.index - 1);
			if (Math.abs(carLocation - (c2.location + c2.length)) < 1) {
				double time = endClock(sensors) - startTime;
				if (time > 1) {
					DC.fitness[cluster] += time;
					startTime = startClock(sensors);
				}
			}
		}

		// Acceleration part	
		if (sensors.getRacePosition() == 1 && counter % 1000 == 0) {
			System.out.println("Accel: " +genes[cluster].accelerationPoint+"\tBrake: " +genes[cluster].brakingPoint +"\tCornerSpeed: " +genes[cluster].cornerSpeed);
		}
		if (cornerStart - carLocation < genes[cluster].brakingPoint && sensors.getSpeed() > genes[cluster].cornerSpeed) {
			return -1;
		} else if (carLocation > cornerStart && carLocation < c.location + genes[cluster].accelerationPoint) {
			return 0.5;
		} else {
			return 1;
		}

	}
	
	private int isCalm(SensorModel sensors) {
		if (sensors.getDistanceFromStartLine() == lastPos) {
			if (sensors.getCurrentLapTime() - posStart > 20) {
				utils.calm(null, sensors);
			}
		} else {
			lastPos = sensors.getDistanceFromStartLine();
			posStart = sensors.getCurrentLapTime();
		}
		return 0;
	}

	public double getSteering3(SensorModel sensors){

		Corner c = trackmap.getNextCorner(sensors.getDistanceFromStartLine());
		if(c.distance > 0){
			// You are approaching a corner
			if(c.sharpness < 0){
				//Corner to the left
				if( sensors.getTrackPosition() < c1 ){
					// Move to the right side of the track
					return utils.moveTowardsTrackPosition( sensors, Math.abs(sensors.getTrackPosition() - c1), c1 );	
				} else {
					// Straight on
					return utils.alignToTrackAxis(sensors, Math.abs(sensors.getAngleToTrackAxis()));
				}
			} else {
				//Corner to the right
				if( sensors.getTrackPosition() > -1 * c1 ){
					// Move to the left side of the track
					return utils.moveTowardsTrackPosition( sensors, Math.abs(sensors.getTrackPosition() - c1), -1 * c1 );	
				} else {
					// Straight on
					return utils.alignToTrackAxis(sensors, Math.abs(sensors.getAngleToTrackAxis()));
				}
			}
		} else {
			// You are in a corner, so move to the track axis
			return utils.moveTowardsTrackPosition(sensors, 2*Math.abs(sensors.getTrackPosition()), 0);
		}

	}

	public double getSteering2(SensorModel sensors) {
		int cluster = getCornerCluster(sensors);
		Corner c = trackmap.getNextCorner(sensors.getDistanceFromStartLine());
		double cornerDistance = c.location  - sensors.getDistanceFromStartLine();
		double speed = sensors.getSpeed();
		double sharpness = c.sharpness;
		double steering = genes[cluster].steeringWeights[0] * speed + genes[cluster].steeringWeights[1] * cornerDistance + genes[cluster].steeringWeights[0]*sharpness;
		return steering;
	}

	public double getSteering(SensorModel sensors) {
		int cluster = getCornerCluster(sensors);
		Corner c = trackmap.getNextCorner(sensors.getDistanceFromStartLine());
		double cornerDistance = c.location  - sensors.getDistanceFromStartLine();
		double speed = sensors.getSpeed();
		double sharpness = c.sharpness;
		double steering = genes[cluster].steeringWeights[0] * speed * 0.0001 + genes[cluster].steeringWeights[1] * cornerDistance * 0.0001 + genes[cluster].steeringWeights[0]*sharpness * 0.0001;
		if (sensors.getRacePosition() == 1 && counter % 1000 == 0) {
			System.out.println("SteeringStrength " +genes[cluster].steeringStrength+"\tSteering: " +steering);
		}
		if (sensors.getDistanceFromStartLine() > c.location + genes[cluster].accelerationPoint && genes[cluster].accelerationPoint <= 0)
			return utils.alignToTrackAxis(sensors, genes[cluster].steeringStrength);
		else if (genes[cluster].accelerationPoint > 0 && sensors.getDistanceFromStartLine() > trackmap.getCorner(c.index-1).distance + genes[cluster].accelerationPoint )
			return utils.alignToTrackAxis(sensors, genes[cluster].steeringStrength);
		if (sharpness < 0)
			return utils.moveTowardsTrackPosition( sensors, steering, -1 * genes[cluster].steeringStrength );
		else
			return utils.moveTowardsTrackPosition( sensors, steering, genes[cluster].steeringStrength );
	}

	public int getCornerCluster(SensorModel sensors) {

		int smallestIdx = 0;
		double shortestDistance = Double.MAX_VALUE;
		Corner c = trackmap.getNextCorner(sensors.getDistanceFromStartLine());
		for (int i = 0; i < DefaultEA.clusters.length; i++) {
			double distance = Math.sqrt( Math.pow(c.sharpness - DefaultEA.clusters[i].sharpness, 2) + Math.pow(c.length - DefaultEA.clusters[i].length, 2) );
			if (distance < shortestDistance) {
				shortestDistance = distance;
				smallestIdx = i;
			}
		}
		return smallestIdx;
	}

	public double startClock(SensorModel sensors) {
		return sensors.getCurrentLapTime();
	}

	public double endClock(SensorModel sensors) {
		return sensors.getCurrentLapTime();
	}

	public String getDriverName() {
		return "Neutrino " +driverName;
	}


	/*
	 * The following methods are only here as a reminder that you can,
	 * and may change all driver methods, including the already implemented
	 * ones, such as those beneath.
	 */
	public void controlQualification(Action action, SensorModel sensors) {	
		super.controlQualification(action, sensors);
	}

	public void controlRace(Action action, SensorModel sensors) {
		super.controlRace(action, sensors);
	}

	public void defaultControl(Action action, SensorModel sensors){
		super.defaultControl(action, sensors);
	}


}
